package com.neuronrobotics.aim.activecannula;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import javax.swing.AbstractButton;
import javax.swing.JButton;
import javax.swing.JLabel;
import javax.swing.JPanel;

import javax.swing.JTextField;

import com.neuronrobotics.aim.gui.MainGUI;
import com.neuronrobotics.sdk.genericdevice.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;

import net.miginfocom.swing.MigLayout;
import java.awt.Color;
import java.awt.Font;

public class ActiveCannulaControlPanel0 extends JPanel implements
		IPIDEventListener {
	private static final long serialVersionUID = 1L;
	protected static final AbstractButton textFieldResetEncoderInnerTranlsation = null;
	private final JLabel lblOuterTubeTranslation = new JLabel(
			"Outer Tube Translation");
	private final JLabel lblDesiredJointPos = new JLabel("Desired Joint Pos");
	private final JLabel lblActualJointPos = new JLabel("Actual Joint Pos");
	private final JLabel lblTime = new JLabel("Time");
	private final JLabel lblMiddleTubeTranslation = new JLabel(
			"Middle Tube Translation");
	private final JTextField textFieldDesiredOuterTranslation = new JTextField();
	private final JTextField textFieldActualOuterTranslation = new JTextField();
	private final JTextField textFieldTimeOuterTranslation = new JTextField();
	private final JTextField textFieldDesiredMiddleTranslation = new JTextField();
	private final JTextField textFieldActualMiddleTranslation = new JTextField();
	private final JTextField textFieldTimeMiddle = new JTextField();
	private final JLabel lblMiddleTubeRotation = new JLabel(
			"Middle Tube Rotation");
	private final JLabel lblInnerTubeRotation = new JLabel(
			"Inner Tube Rotation");
	private final JTextField textFieldDesiredMiddleRotation = new JTextField();
	private final JTextField textFieldDesiredInnerRotation = new JTextField();
	private final JTextField textFieldActualMiddleRotation = new JTextField();
	private final JTextField textFieldActualInnerTranslation = new JTextField();
	private final JTextField textFieldTimeInner = new JTextField();
	private final JButton btnStop = new JButton("STOP");
	private final JButton btnMoveOuterTranslation = new JButton("Move");
	private final JButton btnMoveMiddle = new JButton("Move");
	private final JButton btnMoveMiddleInner = new JButton("Move");
	private final JButton btnHome = new JButton("Home");
	private final JTextField textFieldResetEncoderOuterTranslation = new JTextField();
	private final JTextField textFieldResetEncoderMiddleTranslation = new JTextField();
	private final JTextField textFieldResetEncoderMiddleRotation = new JTextField();
	private final JTextField textFieldResetEncoderInnerRotation = new JTextField();
	private final JButton buttonJogPlusOuterTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusOuterTranslation = new JButton("Jog-");
	private final JButton buttonJogPlusMiddleTranslation = new JButton("Jog+");
	private final JButton buttonJogPlusMiddleRotation = new JButton("Jog+");
	private final JButton buttonJogPlusInnerTranslation = new JButton("Jog+");
	private final JButton buttonJogMinusMiddleTranslation = new JButton("Jog-");
	private final JButton buttonJogMinusMiddleRotation = new JButton("Jog-");
	private final JButton buttonJogMinusInnerTranslation = new JButton("Jog-");
	

	// Linear encoder convention ratio: 1 inch has 500 ticks==0.02032
	private final double encoderTick2mm= 0.0508;
	// Rotary encoder convention ratio: 1 round has 1250 ticks=360/1250=0.288
	private final double mm2EncoderTick =19.6850394;
	
	private final double encoderTicks2degree= 0.288;
	private final double degree2EncoderTick= 3.47222222;
	
	
	private int[] currentJointTicksValues = null;
	private int[] JointJoggingTicks = null;
	private final JTextField textFieldJogStep = new JTextField();
	private final JLabel labelJogStepUnits = new JLabel("mm/deg");
	private int[] desiredJointTicks = null;
	
	public int[] desiredJointTicksInteger = new int[5];
	
	
	public int calibratedOutTranslate;
	public int calibratedMiddleTranslate;
	public int calibratedMiddleRotate;
	public int calibratedInnerTranslate;

	private MainGUI kinematicsGUI;
	private final JLabel lblOffset = new JLabel("Offset");
	private final JLabel lblAxis = new JLabel("Axis 0");
	private final JLabel lblAxis_1 = new JLabel("Axis 1");
	private final JLabel lblAxis_2 = new JLabel("Axis 3");
	private final JLabel lblAxis_3 = new JLabel("Axis 4");
	private final JButton btnResetEncoder = new JButton("Reset");
	

	
	public ActiveCannulaControlPanel0(MainGUI kinematicsGUI) {
		textFieldResetEncoderMiddleRotation.setText("0");
		textFieldResetEncoderMiddleRotation.setColumns(10);
		textFieldResetEncoderMiddleTranslation.setText("0");
		textFieldResetEncoderMiddleTranslation.setColumns(10);
		textFieldResetEncoderOuterTranslation.setText("0");
		textFieldResetEncoderOuterTranslation.setColumns(10);
		setLayout(new MigLayout("", "[22px][][39px][::60px,grow][:30.00:60px][][94.00][][::85.00px,grow][][][][]", "[20px][][][][][][][][][][][:56.00:30px][][]"));
		
		add(lblOffset, "cell 3 1,alignx center");
		textFieldJogStep.setText("1");
		textFieldJogStep.setColumns(10);
		
		add(textFieldJogStep, "cell 4 1,alignx center");
		
		add(labelJogStepUnits, "cell 5 1,alignx center");
		add(lblDesiredJointPos, "cell 6 1,alignx center");
		add(lblActualJointPos, "cell 8 1,alignx center");
		add(lblTime, "cell 10 1,alignx center");
		
		add(lblAxis, "cell 1 2");
		lblOuterTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		lblOuterTubeTranslation.setForeground(Color.BLUE);
		add(lblOuterTubeTranslation, "cell 2 2,alignx trailing");

		add(textFieldResetEncoderOuterTranslation, "cell 3 2,growx");
		btnMoveOuterTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {	
				double seconds = Double.parseDouble(textFieldTimeOuterTranslation.getText());
				runDesiredJointChannelUpdate(0, seconds);
			}
		});
		buttonJogPlusOuterTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderOuterTranslation =Double.parseDouble(textFieldResetEncoderOuterTranslation.getText()); 
				double encoderOuterTranslationJog =encoderOuterTranslation+jogStep;
				textFieldResetEncoderOuterTranslation.setText(Double.toString(encoderOuterTranslationJog));
				
				int encoderOuterTranslationJogStepTick =(int) (encoderOuterTranslationJog*mm2EncoderTick);
				
				setJointJoggingTick(0,encoderOuterTranslationJogStepTick);						
				runJointJoggingTicksUpdate(0, 1);
				
			}
		});

		add(buttonJogPlusOuterTranslation, "cell 4 2");
		buttonJogMinusOuterTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
			
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderOuterTranlsation =Double.parseDouble(textFieldResetEncoderOuterTranslation.getText()); 
				double encoderOuterTranlsationJog =encoderOuterTranlsation-jogStep;
				textFieldResetEncoderOuterTranslation.setText(Double.toString(encoderOuterTranlsationJog));
				
				int encoderOuterTranlsationJogStepTick =(int) (encoderOuterTranlsationJog*mm2EncoderTick);
				
				setJointJoggingTick(0,encoderOuterTranlsationJogStepTick);						
				runJointJoggingTicksUpdate(0, 1);
				
			}
		});

		add(buttonJogMinusOuterTranslation, "cell 5 2");
		textFieldDesiredOuterTranslation.setText("0");
		textFieldDesiredOuterTranslation.setColumns(10);
		add(textFieldDesiredOuterTranslation, "cell 6 2,alignx left");

		textFieldDesiredOuterTranslation
				.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
		textFieldActualOuterTranslation.setEditable(false);
		textFieldActualOuterTranslation.setText("0");
		textFieldActualOuterTranslation.setColumns(10);

		add(textFieldActualOuterTranslation, "cell 8 2");
		textFieldTimeOuterTranslation.setText("3");
		textFieldTimeOuterTranslation.setColumns(10);
		add(textFieldTimeOuterTranslation, "cell 10 2");
		add(btnMoveOuterTranslation, "cell 12 2");
		
		add(lblAxis_1, "cell 1 4");
		lblMiddleTubeTranslation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		lblMiddleTubeTranslation.setForeground(Color.BLUE);
		add(lblMiddleTubeTranslation, "cell 2 4,alignx trailing");

		add(textFieldResetEncoderMiddleTranslation, "cell 3 4,growx");

		textFieldDesiredMiddleTranslation
				.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
					}
				});
		buttonJogPlusMiddleTranslation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double encoderMiddleTranlsation =Double.parseDouble(textFieldResetEncoderMiddleTranslation.getText()); 
				double encoderOuterTranlsationJog =encoderMiddleTranlsation+jogStep;
				textFieldResetEncoderOuterTranslation.setText(Double.toString(encoderOuterTranlsationJog));
				
				int encoderOuterTranlsationJogStepTick =(int) (encoderOuterTranlsationJog*mm2EncoderTick);
				
				setJointJoggingTick(0,encoderOuterTranlsationJogStepTick);						
				runJointJoggingTicksUpdate(0, 1);
				
			}
		});

		add(buttonJogPlusMiddleTranslation, "cell 4 4");

		add(buttonJogMinusMiddleTranslation, "cell 5 4");
		textFieldDesiredMiddleTranslation.setText("0");
		textFieldDesiredMiddleTranslation.setColumns(10);
		add(textFieldDesiredMiddleTranslation, "cell 6 4,growx");
		textFieldActualMiddleTranslation.setEditable(false);
		textFieldActualMiddleTranslation.setText("0");
		textFieldActualMiddleTranslation.setColumns(10);
		add(textFieldActualMiddleTranslation, "cell 8 4,growx");
		btnMoveMiddle.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double seconds = Double.parseDouble(textFieldTimeMiddle.getText());
				runDesiredJointChannelUpdate(1, seconds);				
				runDesiredJointChannelUpdate(3, seconds);
				
			}
		});
		textFieldTimeMiddle.setText("3");
		textFieldTimeMiddle.setColumns(10);
		add(textFieldTimeMiddle, "cell 10 4,growx");
		add(btnMoveMiddle, "cell 12 5");
		
		add(lblAxis_2, "cell 1 6");
		lblMiddleTubeRotation.setForeground(Color.BLUE);
		lblMiddleTubeRotation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		add(lblMiddleTubeRotation, "cell 2 6,alignx center");

		add(textFieldResetEncoderMiddleRotation, "cell 3 6,growx");

		textFieldDesiredMiddleRotation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
			}
		});

		add(buttonJogPlusMiddleRotation, "cell 4 6");

		add(buttonJogMinusMiddleRotation, "cell 5 6");
		textFieldDesiredMiddleRotation.setText("0");
		textFieldDesiredMiddleRotation.setColumns(10);
		add(textFieldDesiredMiddleRotation, "cell 6 6,growx");
		textFieldActualMiddleRotation.setEditable(false);
		textFieldActualMiddleRotation.setText("0");
		textFieldActualMiddleRotation.setColumns(10);
		add(textFieldActualMiddleRotation, "cell 8 6,growx");
		
		add(lblAxis_3, "cell 1 8");
		lblInnerTubeRotation.setForeground(Color.BLUE);
		lblInnerTubeRotation.setFont(new Font("Tahoma", Font.PLAIN, 15));
		add(lblInnerTubeRotation, "cell 2 8,alignx center");
		textFieldResetEncoderInnerRotation.setText("0");
		textFieldResetEncoderInnerRotation.setColumns(10);

		add(textFieldResetEncoderInnerRotation, "cell 3 8,growx");

		btnMoveMiddleInner.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double seconds = Double.parseDouble(textFieldTimeInner.getText());
				runDesiredJointChannelUpdate(4, seconds);					
			}
		});

		textFieldDesiredInnerRotation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
			}
		});

		add(buttonJogPlusInnerTranslation, "cell 4 8");

		add(buttonJogMinusInnerTranslation, "cell 5 8");
		textFieldDesiredInnerRotation.setText("0");
		textFieldDesiredInnerRotation.setColumns(10);
		add(textFieldDesiredInnerRotation, "cell 6 8,growx");
		textFieldActualInnerTranslation.setEditable(false);
		textFieldActualInnerTranslation.setText("0");
		textFieldActualInnerTranslation.setColumns(10);
		add(textFieldActualInnerTranslation, "cell 8 8,growx");
		textFieldTimeInner.setText("3");
		textFieldTimeInner.setColumns(10);
		add(textFieldTimeInner, "cell 10 8,growx");

		add(btnMoveMiddleInner, "cell 12 8");
				btnResetEncoder.addActionListener(new ActionListener() {
					public void actionPerformed(ActionEvent e) {
						calibratedOutTranslate= (int)(Double.parseDouble(textFieldResetEncoderOuterTranslation.getText())*mm2EncoderTick);
						calibratedMiddleTranslate= (int)(Double.parseDouble(textFieldResetEncoderMiddleTranslation.getText())*mm2EncoderTick);
						calibratedMiddleRotate= (int)(Double.parseDouble(textFieldResetEncoderMiddleRotation.getText())*degree2EncoderTick);
						calibratedInnerTranslate= (int)(Double.parseDouble(textFieldResetEncoderInnerTranlsation.getText())*mm2EncoderTick);
						
					}
				});
				
				add(btnResetEncoder, "cell 4 11");
				btnStop.setForeground(Color.RED);
				btnStop.setFont(new Font("Tahoma", Font.BOLD, 15));
				btnStop.setBounds(91, 332, 80, 55);
				
						btnStop.addActionListener(new ActionListener() {
							public void actionPerformed(ActionEvent e) {
								
								getDevice().stopHeartBeat();
//								for(PIDControlWidget w:widgits) {
//									w.stopPID();
//								}
								
							}
						});
						btnHome.setFont(new Font("Tahoma", Font.PLAIN, 15));
						add(btnHome, "cell 6 11,alignx center");
						add(btnStop, "cell 8 11,alignx center");

	}

	
	@Override
	public void onPIDEvent(PIDEvent e) {
		System.out
		.println("Got a PID event in Kinematics GUI #" + e.getGroup()
				+ " value: " + e.getValue() + " timestamp: "
				+ e.getTimeStamp());
		// getCurrentJointTicksValues() is 4x1 vector
		getCurrentJointTicksValues()[e.getGroup()] = e.getValue();
		updateRobotJointDisplay(getCurrentJointTicksValues());
	}

	@Override
	public void onPIDLimitEvent(PIDLimitEvent e) {
		
	}

	@Override
	public void onPIDReset(int group, int currentValue) {

	}
	
	public void updateRobotJointDisplay (int[] currentJointTicks){
		double outerTranslation = getCurrentJointTicksValues()[0]*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(outerTranslation));
		
		double middleTranslation = getCurrentJointTicksValues()[1]*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(middleTranslation));
		
		double middleRotation = getCurrentJointTicksValues()[3]*encoderTicks2degree;
		textFieldActualOuterTranslation.setText(Double.toString(middleRotation));
		
		double innerTranslation = getCurrentJointTicksValues()[4]*encoderTick2mm;
		textFieldActualOuterTranslation.setText(Double.toString(innerTranslation));
		
//		// this would set up the errors
//		double outerTranslationError= Double.parseDouble(textFieldDesiredOuterTranslation.getText())-outerTranslation;		
	}

	public void setCurrentJointTicksValues(int[] currentJointTicksValues) {
		this.currentJointTicksValues = currentJointTicksValues;
	}

	public int[] getCurrentJointTicksValues() {
		return currentJointTicksValues;
	}
	
	public synchronized void runJointJoggingTicksUpdate(int channel, double seconds) {
		getDevice().SetPIDSetPoint(channel, getJointJoggingTicks()[channel],seconds);
	}
	
	public GenericPIDDevice getDevice() {
		return kinematicsGUI.getDevice();
	}


	public void setJointJoggingTicks(int[] jointJoggingTicks) {
		JointJoggingTicks = jointJoggingTicks;
	}


	public int[] getJointJoggingTicks() {
		return JointJoggingTicks;
	}
	
	public void setJointJoggingTick(int channel , int jointJoggingTick) {
		JointJoggingTicks[channel] = jointJoggingTick;
	}
	
	public synchronized void runDesiredJointUpdate(double seconds) {
		// 1 step: update desired joint
		// reference the jointSpaceVector (integer: encoder ticks)
		setDesiredJointTickValue(0,desiredJointTicksInteger[0]-calibratedOutTranslate);
		setDesiredJointTickValue(1,desiredJointTicksInteger[1]-calibratedMiddleTranslate);
		setDesiredJointTickValue(2,desiredJointTicksInteger[2]);
		setDesiredJointTickValue(3,desiredJointTicksInteger[3]-calibratedMiddleRotate);	
		setDesiredJointTickValue(4,desiredJointTicksInteger[4]-calibratedInnerTranslate);	
		
		System.err.println("Updating motors");
		getDevice().SetPIDSetPoint(0, getDesiredJointTicksValues()[0],seconds);
		getDevice().SetPIDSetPoint(1, getDesiredJointTicksValues()[1],seconds);
		getDevice().SetPIDSetPoint(2, getDesiredJointTicksValues()[2],seconds);
		getDevice().SetPIDSetPoint(3, getDesiredJointTicksValues()[3],seconds);
		getDevice().SetPIDSetPoint(4, getDesiredJointTicksValues()[4],seconds);	
	}
	
	
	public synchronized void runDesiredJointChannelUpdate(int channel, double seconds) {
		int calibrated=0;
		
		 switch (channel) {
		 case 0:  calibrated = calibratedOutTranslate;     //  break;
         case 1:  calibrated = calibratedMiddleTranslate;    //   break;
         case 2:  calibrated = 0;   //   break;
         case 3:  calibrated = calibratedMiddleRotate;       //  break;
         case 4:  calibrated = calibratedInnerTranslate;      //   break;
         
         default: calibrated = 0; //break;
     }
		 	 
		setDesiredJointTickValue(channel,desiredJointTicksInteger[channel]-calibrated);
		
		System.err.println("Updating motors");
		getDevice().SetPIDSetPoint(calibrated, getDesiredJointTicksValues()[calibrated],seconds);		
	}
	
	
	
	public void setDesiredJointTickValue(int group, int target) {
		getDesiredJointTicksValues()[group] = target;
	}
	
	public int[] getDesiredJointTicksValues() {
		return desiredJointTicks;
	}
	
}
